/*
 * CoarseAlignmentSM.c
 *
 *  Created on: Nov 30, 2012
 *      Author: gmkado
 */

/*****************************************************
 * Include Statements
 ****************************************************/
#include "CoarseAlignmentSM.h"
#include "hw_types.h"
#include "inc/lm3s2965.h"
#include "URF.h"
#include "Servo.h"
#include <stdio.h>
#include <stdlib.h>
#include "CoarseLocalization.h"
#include <math.h>

//TODO: CALIBRATE THESE
#define RANGE_JUMP_THRESHOLD 15
#define HOME_DIFF_TOL 15			// WHEN GOING BACK HOME AFTER FINDING DRIVER SIDE, CHECK IF WE'RE GETTING THE SAME RANGE READING
#define URF_ANGLE_OFFSET 20
#define STEP_ANGLE 1
#define X_MIN
/*****************************************************
 * Module Level Variables
 ****************************************************/
// Module level variable contains the current state
static CoarseAlignmentSMState_t currentState;
static range_t currRange, driverRange, passengerRange, homeRange;
static angle_t currAngle, driverAngle, passengerAngle;

static tBoolean adjustAngleOffset(void);

/*****************************************************
 * Initialization Function
 ****************************************************/
void InitCoarseAlignmentSM(void)
{
	// All initialization connected to this SM goes here
	currentState = SetHomeRange;
	puts("CoarseAlignment: Initializing");
	initURF();
	initServo();
	goHome();
}

/*****************************************************
 * Run Function
 ****************************************************/
char RunCoarseAlignmentSM(void)
{
	angle_t prevAngle;
	range_t prevRange;

	prevAngle = currAngle;
	prevRange = currRange;

	switch(currentState)
	{
		case SetHomeRange:
			homeRange = getURFRange();
			currRange = homeRange;
			currentState = FindDriverSide;
			puts("CoarseAlignment:Finding Driver Side");
		break;
		case FindDriverSide:
			/* move CW until a jump is seen or servo angle passes 0 degrees */
			moveServoCW(STEP_ANGLE);

			currAngle = getServoAngle();
			currRange = getURFRange();

			printf("CoarseAlignment: CurrAngle = %.1f degrees, CurrRange = %.1f cm\n", currAngle, currRange);
			printf("currRange - prevRange = %.1f\n", currRange - prevRange);

			if(currAngle<MIN_ANGLE)
			{
				// never got a range jump, report vehicle out of range
				puts("Never found driver side");
				while(1){}
				// TODO: Throw OUT_OF_RANGE event
			}
			else if(currRange-prevRange > RANGE_JUMP_THRESHOLD)
			{
				// distance increased, found an edge
				driverRange = prevRange;
				driverAngle = prevAngle; // + URF_ANGLE_OFFSET;
				currentState = GoBackHome;
				puts("CoarseAlignment:Going Back Home");
				goHome();
			} else if(currRange-prevRange < -RANGE_JUMP_THRESHOLD)
			{
				// distance decreased, for now just block until it leaves

				do{
					puts("Something's in the way");
					currRange = getURFRange();
				}while(currRange - prevRange < -RANGE_JUMP_THRESHOLD);
			}

		break;
		case GoBackHome:
			// check if we've gotten back to the home range
			currRange = getURFRange();
			if(abs(currRange - homeRange) <  HOME_DIFF_TOL) {
				// we've gotten back home, so start finding passenger side
				currentState = FindPassengerSide;
				puts("CoarseAlignment: Finding Passenger Side");
			}
		break;
		case FindPassengerSide:

			/* move CW until a jump is seen or servo angle passes 0 degrees */
			moveServoCCW(STEP_ANGLE);

			currAngle = getServoAngle();
			currRange = getURFRange();

			printf("CoarseAlignment: CurrAngle = %.1f degrees, CurrRange = %.1f cm\n", currAngle, currRange);
			printf("currRange - prevRange = %.1\nf", currRange - prevRange);

			if(currAngle>MAX_ANGLE)
			{
				// never got a range jump, report vehicle out of range
				puts("Never found passenger side");
				while(1){}
				// TODO: Throw OUT_OF_RANGE event

			}
			else if(currRange-prevRange > RANGE_JUMP_THRESHOLD)
			{
				passengerRange = prevRange;
				passengerAngle = prevAngle; // - URF_ANGLE_OFFSET;
				currentState = SetHomeRange;

				goHome();

				printf("CoarseAlignment: Finished!  driver range = %.1f, angle = %.1f, passenger range = %.1f, angle = %.1f\n", driverRange,driverAngle, passengerRange, passengerAngle);
				puts("CoarseAlignment: Returning to SetHomeRange");
				coarseLocalize( driverRange, passengerRange, driverAngle, passengerAngle);
				MoveToCoord(getXCarCoarse(), getYCarCoarse(), 0);

				return 1;

			}
			else if(currRange-prevRange < -RANGE_JUMP_THRESHOLD)
			{
				// distance decreased, for now just block until it leaves

				do{
					puts("Something's in the way");
					currRange = getURFRange();
				}while(currRange - prevRange < -RANGE_JUMP_THRESHOLD);
			}
		break;
	}

	return 0;
}

static tBoolean adjustAngleOffset(void)
{
	// Adjust angles so they conform with known width of car
	// Do this by incrementing (or decrementing) the angles until the length between the two points equals the carwidth
	// This function should only be called within this module

#define CAR_WIDTH 		121.9
#define RAW_WIDTH_TOL 	50
#define WIDTH_TOL  		1


	float detectedWidth = polarDist(driverRange, passengerRange, driverAngle, passengerAngle);
	printf("CoarseAlignment:Detected width = %f\n", detectedWidth);
	if(abs(CAR_WIDTH - detectedWidth) > RAW_WIDTH_TOL)
	{
		puts ("CoarseAlignment:Detected width is outside tolerance");
		return false; // exited with error
	}else
	{
		// calculate the angle offset
		while(abs(CAR_WIDTH - detectedWidth) > WIDTH_TOL)
		{
			if(detectedWidth>CAR_WIDTH)
			{
				driverAngle++;
				passengerAngle--;
			}else
			{
				driverAngle--;
				passengerAngle++;
			}
			detectedWidth = polarDist(driverRange, passengerRange, driverAngle, passengerAngle);
			printf("CoarseAlignment:Width adjusted: New width = %f\n", detectedWidth);
		}
		printf("CoarseAlignment:Width successfully adjusted!  driver range = %.1f, angle = %.1f, passenger range = %.1f, angle = %.1f\n", driverRange,driverAngle, passengerRange, passengerAngle);
		return true;  // exited successfully
	}
}

float polarDist(float r1, float r2, float th1, float th2)
{
#define DEG_TO_RAD 3.14/180
	// return distance between two polar coordinates
	return cartDist(r1*cos(th1*DEG_TO_RAD), r1*sin(th1*DEG_TO_RAD), r2*cos(th2*DEG_TO_RAD), r2*sin(th2*DEG_TO_RAD));
}

float cartDist(float x1, float y1, float x2, float y2)
{
	return sqrt(pow(x1-x2,2) + pow(y1-y2,2));
}

void goHome(void)
{
	moveServo(SERVO_HOME);
	currAngle = SERVO_HOME;
}

char ValidateCoordinates(void){
	float x = getXCarCoarse();
	float y = getYCarCoarse();

	if((x > 40 || x < 0) || (y > 40 || y < 0)){
		return 0;
	}

	return 1;
}
